#define _GNU_SOURCE 1
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <getopt.h>

#include "ukfCfg.h"
#include "ukfCfg1.h"

// #define cfg0 (int)0
#define cfg1 (int)1

/*---------------------------------------------*/
/*           Function Prototype                */
/*---------------------------------------------*/
int ukf_test(void);
// int verbose = 0;

// int main(int argc, char *argv[])
int main()
{
	int error = 0;
 	error |= ukf_test();
	return 0;
}

/**
 *
 * Initialize and test UKF C implementation against expected result. Filter is tested in the loop from 15 steps.
 * 根据预期结果初始化和测试 UKF C 实现。 滤波器在 15 个步骤的循环中进行测试。
 * Total root square error is accumulated in the same loop for each state in order to show deviation from reference matlab solution.
 * 总平方根误差在每个状态的相同循环中累积，以显示与参考 matlab 解决方案的偏差。
 */
int ukf_test(void)
{
	_Bool tfInitCfg1 = 0;
	UKF_t ukfIo[2];
	uint32_t simLoop;

 	// UKF initialization: CFG1(free pendulum) 自由摆
	tfInitCfg1 = ukf_init(&ukfIo[cfg1], &UkfMatrixCfg1);

	if (tfInitCfg1 == 0)
	{
		static double tetha = 0.5;   // initial conditions for angle 		初始角度
		static double tetha_dot = 0; // initial conditions for angle speed	初始角速度
		const double B = 0.05;		 // kg*s/m	kg*s/m 是动量的单位，通常也称为牛顿秒（N·s）。
		const double l = 0.613;      // 摆杆长度
		const double m = 0.5;		 // 摆杆质量
		const double g = 9.81;		 // 重力加速度
		static const double T0 = 0.0001;

		// UKF simulation: BEGIN
		for (simLoop = 0; simLoop < 70; simLoop++)
		{
			double *const py_cfg1 = ukfIo[cfg1].input.y.val;
			// double err[2] = { 0, 0 };

			// UKF:CFG1 apply/load system measurements in working array for current iteration
			// UKF:CFG1 在当前迭代的工作阵列中应用/加载系统测量

			tetha = tetha + T0 * tetha_dot;
			tetha_dot = tetha_dot - ((T0 * B * tetha_dot) / m) - ((T0 * g) / l) * sin(tetha);

			py_cfg1[0] = tetha;

			// UKF:CFG0 periodic task call
			(void)ukf_step(&ukfIo[cfg1]);

			// err[0] = fabs(ukfIo[cfg1].update.x.val[0] - tetha);
			// err[1] = fabs(ukfIo[cfg1].update.x.val[1] - tetha_dot);

			// printf("Loop: %d |system states : real | system states : est | system states : err \n", (int)simLoop);
			// printf("          %2.14f       %2.14f      %2.14f\n", 	tetha, 		ukfIo[1].update.x.val[0], err[0]);
			// printf("          %2.14f      %2.14f     %2.14f\n",		tetha_dot, 	ukfIo[1].update.x.val[1], err[1]);

			// printf("Loop: %d |system states : real | system states : est | system states : err \n", (int)simLoop);
			// printf("%2.14f,	%2.14f.\n", 	tetha, 		ukfIo[1].update.x.val[0]);
			// printf("%2.14f,	%2.14f.\n",		tetha_dot, 	ukfIo[1].update.x.val[1]);
		}
		// UKF simulation: END
	}
	else
	{
		// initialization fail
		// TBD
		return -1;
	}
	return 0;
}